// --------------------------------------------------------------------------------------------------------------------
// <copyright file="JointController.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Manipulation.JointController
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Manipulation.JointController;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Tests for JointController abstract class
    /// All tests use MockJointController.
    /// </summary>
    [TestClass]
    public class JointControllerTests
    {
        /// <summary>
        /// Test constructor
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void TestJointControllerConstructor()
        {
            int numJoints = 8;

            JointController jointController = new MockJointController(numJoints);

            Assert.AreEqual(numJoints, jointController.NumJoints);
            Assert.AreEqual(false, jointController.IsControllerDeactivated);
        }

        /// <summary>
        /// Test setting reference values
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void TestJointControllerSetReferenceValues()
        {
            int numJoints = 7;

            JointController jointController = new MockJointController(numJoints);

            double[] referenceValues = { 0.0, 2.0, 4.0, 6.0, 8.0, 10.0, 12.0 };

            jointController.SetReferences(referenceValues);

            double[] jointControllerReferenceValues = jointController.References;

            // check that read values are the values set
            for (int i = 0; i < numJoints; ++i)
            {
                Assert.AreEqual(referenceValues[i], jointControllerReferenceValues[i]);
            }

            // check that values we just read are unchanged even if new values are sent
            // to the controller (e.g. testing that we got a reference to a copy of the
            // values, not the internal array in the controller)

            double[] newReferenceValues = { 1.0, 3.0, 5.0, 7.0, 9.0, 11.0, 13.0 };

            jointController.SetReferences(newReferenceValues);

            double[] newJointControllerReferenceValues = jointController.References;

            // check that the original and new values are distinct
            for (int i = 0; i < numJoints; ++i)
            {
                Assert.AreNotEqual(jointControllerReferenceValues[i], newJointControllerReferenceValues[i]);
            }
        }

        /// <summary>
        /// Test setting absmax values
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void TestJointControllerSetAbsoluteMaximumValues()
        {
            int numJoints = 7;

            JointController jointController = new MockJointController(numJoints);

            double[] expectedValues = { 0.0, 2.0, 4.0, 6.0, 8.0, 10.0, 12.0 };

            jointController.SetMaxAbsOutput(expectedValues);

            double[] jointControllerMaxAbsOutputValues = jointController.MaxAbsOutput;

            // check that read values are the values set
            for (int i = 0; i < numJoints; ++i)
            {
                Assert.AreEqual(expectedValues[i], jointControllerMaxAbsOutputValues[i]);
            }

            // check that values we just read are unchanged even if new values are sent
            // to the controller (e.g. testing that we got a reference to a copy of the
            // values, not the internal array in the controller)

            double[] newValues = { 1.0, 3.0, 5.0, 7.0, 9.0, 11.0, 13.0 };

            jointController.SetMaxAbsOutput(newValues);

            double[] newJointControllerMaxAbsOutputValues = jointController.MaxAbsOutput;

            // check that the original and new values are distinct
            for (int i = 0; i < numJoints; ++i)
            {
                Assert.AreNotEqual(jointControllerMaxAbsOutputValues[i], newJointControllerMaxAbsOutputValues[i]);
            }
        }

        /// <summary>
        /// Test deactivating controller
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        [ExpectedException(typeof(ControllerDeactivatedException))]
        public void TestJointControllerDeactivate()
        {
            int numJoints = 7;

            JointController jointController = new MockJointController(numJoints);

            double[] referenceValues = { 0.0, 2.0, 4.0, 6.0, 8.0, 10.0, 12.0 };

            Assert.IsFalse(jointController.IsControllerDeactivated);

            jointController.DeactivateController();

            Assert.IsTrue(jointController.IsControllerDeactivated);

            jointController.SetReferences(referenceValues);
        }
    }
}
